const mqtt = require("mqtt");
const rclnodejs = require("rclnodejs");
const protobuf = require("protobufjs");
const path = require("path");

// 连接到 MQTT Broker
const mqttBroker = "mqtt://broker-cn.emqx.io:1883/mqtt"; //  mqtt://localhost:1883
const options = { clean: true };
const client = mqtt.connect(mqttBroker, options);

// console.log(rclnodejs);

rclnodejs
  .init()
  .then(() => {
    console.log("ROS 2 rclnodejs initialized");

    const rosbridgeUrl = "ws://192.168.1.216:9090";
    rclnodejs.connect(rosbridgeUrl);
    // 初始化 ROS 2 节点
    const node = rclnodejs.createNode("mqtt");
    const publisher = node.createPublisher("std_msgs/msg/String", "ros_topic");
    //订阅ros_topic话题,将接收到的消息发布到my/topic话题
    node.createSubscription("std_msgs/msg/String", "ros_topic", (msg) => {
      client.publish("my/topic", msg.data);
    });

    // 连接到 MQTT Broker
    client.on("connect", () => {
      console.log("Connected to MQTT Broker");

      // 订阅主题
      client.subscribe("my1/topic", (err) => {
        if (!err) {
          console.log("Subscribed to topic: my1/topic");
        }
      });
    });

    // 处理 MQTT 消息
    client.on("message", (topic, message) => {
      console.log(`Received message on topic ${topic}`);
      // // 创建 ROS 2 消息
      // const rosMessage = { data: message.toString() };
      // // 发布到 ROS 2
      // publisher.publish(rosMessage);

      // 加载 .proto 文件
      const protoPath = path.join(__dirname, "../public/proto/message.proto");
      //使用protobuf编码
      protobuf.load(protoPath, (err, root) => {
        if (err) {
          console.log(`Fail to load proto file: ${err}`);
        }
        const SystemInfo = root.lookupType("SystemInfo");
        try {
          console.log(`message to encode: ${message}`);
          // 根据message创建SystemInfo消息实例
          const rosMessage = SystemInfo.create(message);
          console.log(`rosMessage: ${rosMessage}`);
          //编码为buffer
          const buffer = SystemInfo.encode(rosMessage).finish();
          console.log(`buffer length: ${buffer.length}`);
          console.log(`buffer: ${buffer}`);
          if (buffer.length > 0) {
            // 发布到 ROS 2
            publisher.publish(buffer);
            console.log(`Published message to topic: ros_topic`);
          } else {
            console.log(`fail to encode valid buffer`);
          }
        } catch (err) {
          console.log(`Fail to encode message: ${err}`);
        }
      });
    });

    // 运行 ROS 2 节点
    // rclnodejs.spin(node);
  })
  .catch((err) => {
    console.error("Error initializing ROS 2 node:", err);
  });
